import rclpy
from rclpy.node import Node
from spirems import Publisher, def_msg
from a2rl_bs_msgs.msg import VectornavIns, ModuleStatusReport, Localization, EgoState, ReferencePath
from rosgraph_msgs.msg import Clock
from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped, TwistWithCovarianceStamped
from sensor_msgs.msg import PointCloud2, Image, NavSatFix, Imu
from vectornav_msgs.msg import AttitudeGroup, CommonGroup, GpsGroup, ImuGroup
from tf2_msgs.msg import TFMessage
from rosgraph_msgs.msg import Clock
# from localization_msgs.msg import MahalanobisData
from nav_msgs.msg import Odometry, Path
from std_msgs.msg import Bool
from eav25_bsu_msgs.msg import (Kistler_Correvit, Bad_Alive, Bad_Misc, Bad_Powertrain_misc, Bad_Powertrain_Press, 
Bad_Powertrain_Temp, Bad_Ride_Front, Bad_Ride_Rear, Bad_Wheel_Load, Bad_Z_Accel_Body, Bosch_Pitch, Bosch_Roll, Bosch_Yaw,
Brake_Disk_Temp, BSU_Ranges_01, BSU_Ranges_02, BSU_Status_01, CBA_Status_FL, CBA_Status_FR, CBA_Status_RL, CBA_Status_RR, 
DiagnosticWord_01, DiagnosticWord_02, EM_Status_01, HL_Msg_01, HL_Msg_02, HL_Msg_03, HL_Msg_04, HL_Msg_05, HL_Msg_06, 
ICE_Status_01, ICE_Status_02, Kistler_Acc_Body, Kistler_Angle_Vel_Body, Kistler_Correvit, Kistler_Distance, Kistler_Pitch_Roll, 
Kistler_Status, Kistler_Vel_Angle, PDUs_Status_01, PIT_Packet_0, PIT_Packet_1, PSA_Status_01, PSA_Status_02, RM_Status_01, 
SW_Versions_01, SW_Versions_02, SW_Versions_03, Tpms_Front, Tpms_Rear, Tyre_Surface_Temp_Front, Tyre_Surface_Temp_Rear, Wheels_Speed_01
)
import time
import threading
import numpy as np
from typing import Union
import argparse
import os
from spirems import Publisher, Subscriber, def_msg, cvimg2sms, BaseNode, get_extra_args


"""
-------------------------------------------------------------------
ROS2 Message               -> SpireMS
-------------------------------------------------------------------
VectornavIns               -> nav_msgs::VectornavIns
Clock                      -> std_msgs::Clock
PoseStamped                -> geometry_msgs::PoseInFrame
PointCloud2                -> memory_msgs::PointCloud
NavSatFix                  -> sensor_msgs::NavSatFix
AttitudeGroup              -> nav_msgs::VectornavAttitudeGroup
CommonGroup                -> nav_msgs::VectornavCommonGroup
GpsGroup                   -> nav_msgs::VectornavGpsGroup
ImuGroup                   -> nav_msgs::VectornavImuGroup
TFMessage                  -> geometry_msgs::FrameTransform
TwistStamped               -> geometry_msgs::Twist
Imu                        -> sensor_msgs::Imu
MahalanobisData            -> nav_msgs::MahalanobisData
ModuleStatusReport         -> vehicle_msgs::ModuleStatusReport
Odometry                   -> nav_msgs::Odometry
Localization               -> nav_msgs::Localization
EgoState                   -> vehicle_msgs::EgoState
PoseWithCovarianceStamped  -> geometry_msgs::PoseWithCovariance
TwistWithCovarianceStamped -> geometry_msgs::TwistWithCovariance
Bool                       -> std_msgs::Boolean
Kistler_Correvit           -> vehicle_msgs::KistlerCorrevit
-------------------------------------------------------------------
"""

class ROS2A2RLTopic2SMSNode(Node, threading.Thread, BaseNode):
    def __init__(
        self,
        job_name: str,
        ip: str = '127.0.0.1',
        port: int = 9094,
        param_dict_or_file: Union[dict, str] = None,
        sms_shutdown: bool = True,
        **kwargs
    ):
        Node.__init__(self, 'ROS2A2RLTopic2SMSNode')
        threading.Thread.__init__(self)
        sms_shutdown = True if sms_shutdown in ['True', 'true', '1', True] else False
        BaseNode.__init__(
            self,
            self.__class__.__name__,
            job_name,
            ip=ip,
            port=port,
            param_dict_or_file=param_dict_or_file,
            sms_shutdown=sms_shutdown,
            **kwargs
        )

        # self.ros_topic = self.get_param("ros_topic", "/sensor/lidar_front/points")
        # simulator
        self.a2rl_vn_ins = self.get_param("a2rl_vn_ins", "/flyeagle/a2rl/vn/ins")
        self.clock = self.get_param("clock", "/clock")
        self.flyeagle_ground_truth = self.get_param("flyeagle_ground_truth", "/flyeagle/ground_truth")
        self.vectornav_nav_sat_fix = self.get_param("vectornav_nav_sat_fix", "/flyeagle/vectornav/nav_sat_fix")
        self.vectornav_raw_attitude = self.get_param("vectornav_raw_attitude", "/vectornav/raw/attitude")
        self.vectornav_raw_common = self.get_param("vectornav_raw_common", "/vectornav/raw/common")
        self.vectornav_raw_gps = self.get_param("vectornav_raw_gps", "/vectornav/raw/gps")
        self.vectornav_raw_gps2 = self.get_param("vectornav_raw_gps2", "/vectornav/raw/gps2")
        self.vectornav_raw_imu = self.get_param("vectornav_raw_imu", "/vectornav/raw/imu")

        # ning
        self.flyeagle_a2rl_observer_ego_loc = self.get_param("flyeagle_a2rl_observer_ego_loc", "/flyeagle/a2rl/observer/ego_loc")
        self.flyeagle_a2rl_observer_ego_state = self.get_param("flyeagle_a2rl_observer_ego_state", "/flyeagle/a2rl/observer/ego_state")
        self.flyeagle_localization_pose_estimator_pose_from_gps = self.get_param(
            "flyeagle_localization_pose_estimator_pose_from_gps",
            "/flyeagle/localization/pose_estimator/pose_from_gps"
        )
        self.flyeagle_localization_twist_estimator_twist_from_ins = self.get_param(
            "flyeagle_localization_twist_estimator_twist_from_ins",
            "/flyeagle/localization/twist_estimator/twist_from_ins"
        )
        self.flyeagle_localization_kistler_status = self.get_param(
            "flyeagle_localization_kistler_status",
            "/flyeagle/localization/kistler_status"
        )
        self.flyeagle_localization_pose_twist_fusion_filter_kinematic_state = self.get_param(
            "flyeagle_localization_pose_twist_fusion_filter_kinematic_state",
            "/flyeagle/localization/pose_twist_fusion_filter/kinematic_state"
        )

        # gao
        self.flyeagle_a2rl_eav25_bsu_bad_alive = self.get_param("flyeagle_a2rl_eav25_bsu_bad_alive", "/flyeagle/a2rl/eav25_bsu/bad_alive")
        self.flyeagle_a2rl_eav25_bsu_bad_misc = self.get_param("flyeagle_a2rl_eav25_bsu_bad_misc", "/flyeagle/a2rl/eav25_bsu/bad_misc")
        self.flyeagle_a2rl_eav25_bsu_bad_powertrain_misc = self.get_param("flyeagle_a2rl_eav25_bsu_bad_powertrain_misc", "/flyeagle/a2rl/eav25_bsu/bad_powertrain_misc")
        self.flyeagle_a2rl_eav25_bsu_bad_powertrain_press = self.get_param("flyeagle_a2rl_eav25_bsu_bad_powertrain_press", "/flyeagle/a2rl/eav25_bsu/bad_powertrain_press")
        self.flyeagle_a2rl_eav25_bsu_bad_powertrain_temp = self.get_param("flyeagle_a2rl_eav25_bsu_bad_powertrain_temp", "/flyeagle/a2rl/eav25_bsu/bad_powertrain_temp")
        self.flyeagle_a2rl_eav25_bsu_bad_ride_front = self.get_param("flyeagle_a2rl_eav25_bsu_bad_ride_front", "/flyeagle/a2rl/eav25_bsu/bad_ride_front")
        self.flyeagle_a2rl_eav25_bsu_bad_ride_rear = self.get_param("flyeagle_a2rl_eav25_bsu_bad_ride_rear", "/flyeagle/a2rl/eav25_bsu/bad_ride_rear")
        self.flyeagle_a2rl_eav25_bsu_bad_wheel_load = self.get_param("flyeagle_a2rl_eav25_bsu_bad_wheel_load", "/flyeagle/a2rl/eav25_bsu/bad_wheel_load")
        self.flyeagle_a2rl_eav25_bsu_bad_z_accel_body = self.get_param("flyeagle_a2rl_eav25_bsu_bad_z_accel_body", "/flyeagle/a2rl/eav25_bsu/bad_z_accel_body")
        self.flyeagle_a2rl_eav25_bsu_bosch_pitch = self.get_param("flyeagle_a2rl_eav25_bsu_bosch_pitch", "/flyeagle/a2rl/eav25_bsu/bosch_pitch")
        self.flyeagle_a2rl_eav25_bsu_bosch_roll = self.get_param("flyeagle_a2rl_eav25_bsu_bosch_roll", "/flyeagle/a2rl/eav25_bsu/bosch_roll")
        self.flyeagle_a2rl_eav25_bsu_bosch_yaw = self.get_param("flyeagle_a2rl_eav25_bsu_bosch_yaw", "/flyeagle/a2rl/eav25_bsu/bosch_yaw")
        self.flyeagle_a2rl_eav25_bsu_brake_disk_temp = self.get_param("flyeagle_a2rl_eav25_bsu_brake_disk_temp", "/flyeagle/a2rl/eav25_bsu/brake_disk_temp")
        self.flyeagle_a2rl_eav25_bsu_bsu_ranges_01 = self.get_param("flyeagle_a2rl_eav25_bsu_bsu_ranges_01", "/flyeagle/a2rl/eav25_bsu/bsu_ranges_01")
        self.flyeagle_a2rl_eav25_bsu_bsu_ranges_02 = self.get_param("flyeagle_a2rl_eav25_bsu_bsu_ranges_02", "/flyeagle/a2rl/eav25_bsu/bsu_ranges_02")
        self.flyeagle_a2rl_eav25_bsu_bsu_status_01 = self.get_param("flyeagle_a2rl_eav25_bsu_bsu_status_01", "/flyeagle/a2rl/eav25_bsu/bsu_status_01")
        self.flyeagle_a2rl_eav25_bsu_cba_status_fl = self.get_param("flyeagle_a2rl_eav25_bsu_cba_status_fl", "/flyeagle/a2rl/eav25_bsu/cba_status_fl")
        self.flyeagle_a2rl_eav25_bsu_cba_status_fr = self.get_param("flyeagle_a2rl_eav25_bsu_cba_status_fr", "/flyeagle/a2rl/eav25_bsu/cba_status_fr")
        self.flyeagle_a2rl_eav25_bsu_cba_status_rl = self.get_param("flyeagle_a2rl_eav25_bsu_cba_status_rl", "/flyeagle/a2rl/eav25_bsu/cba_status_rl")
        self.flyeagle_a2rl_eav25_bsu_cba_status_rr = self.get_param("flyeagle_a2rl_eav25_bsu_cba_status_rr", "/flyeagle/a2rl/eav25_bsu/cba_status_rr")
        self.flyeagle_a2rl_eav25_bsu_diagnostic_word_01 = self.get_param("flyeagle_a2rl_eav25_bsu_diagnostic_word_01", "/flyeagle/a2rl/eav25_bsu/diagnostic_word_01")
        self.flyeagle_a2rl_eav25_bsu_diagnostic_word_02 = self.get_param("flyeagle_a2rl_eav25_bsu_diagnostic_word_02", "/flyeagle/a2rl/eav25_bsu/diagnostic_word_02")
        self.flyeagle_a2rl_eav25_bsu_em_status_01 = self.get_param("flyeagle_a2rl_eav25_bsu_em_status_01", "/flyeagle/a2rl/eav25_bsu/em_status_01")
        self.flyeagle_a2rl_eav25_bsu_hl_msg_01 = self.get_param("flyeagle_a2rl_eav25_bsu_hl_msg_01", "/flyeagle/a2rl/eav25_bsu/hl_msg_01")
        self.flyeagle_a2rl_eav25_bsu_hl_msg_02 = self.get_param("flyeagle_a2rl_eav25_bsu_hl_msg_02", "/flyeagle/a2rl/eav25_bsu/hl_msg_02")
        self.flyeagle_a2rl_eav25_bsu_hl_msg_03 = self.get_param("flyeagle_a2rl_eav25_bsu_hl_msg_03", "/flyeagle/a2rl/eav25_bsu/hl_msg_03")
        self.flyeagle_a2rl_eav25_bsu_hl_msg_04 = self.get_param("flyeagle_a2rl_eav25_bsu_hl_msg_04", "/flyeagle/a2rl/eav25_bsu/hl_msg_04")
        self.flyeagle_a2rl_eav25_bsu_hl_msg_05 = self.get_param("flyeagle_a2rl_eav25_bsu_hl_msg_05", "/flyeagle/a2rl/eav25_bsu/hl_msg_05")
        self.flyeagle_a2rl_eav25_bsu_hl_msg_06 = self.get_param("flyeagle_a2rl_eav25_bsu_hl_msg_06", "/flyeagle/a2rl/eav25_bsu/hl_msg_06")
        self.flyeagle_a2rl_eav25_bsu_ice_status_01 = self.get_param("flyeagle_a2rl_eav25_bsu_ice_status_01", "/flyeagle/a2rl/eav25_bsu/ice_status_01")
        self.flyeagle_a2rl_eav25_bsu_ice_status_02 = self.get_param("flyeagle_a2rl_eav25_bsu_ice_status_02", "/flyeagle/a2rl/eav25_bsu/ice_status_02")
        self.flyeagle_a2rl_eav25_bsu_kistler_acc_body = self.get_param("flyeagle_a2rl_eav25_bsu_kistler_acc_body", "/flyeagle/a2rl/eav25_bsu/kistler_acc_body")
        self.flyeagle_a2rl_eav25_bsu_kistler_angle_vel_body = self.get_param("flyeagle_a2rl_eav25_bsu_kistler_angle_vel_body", "/flyeagle/a2rl/eav25_bsu/kistler_angle_vel_body")
        self.flyeagle_a2rl_eav25_bsu_kistler_correvit = self.get_param("flyeagle_a2rl_eav25_bsu_kistler_correvit", "/flyeagle/a2rl/eav25_bsu/kistler_correvit")
        self.flyeagle_a2rl_eav25_bsu_kistler_distance = self.get_param("flyeagle_a2rl_eav25_bsu_kistler_distance", "/flyeagle/a2rl/eav25_bsu/kistler_distance")
        self.flyeagle_a2rl_eav25_bsu_kistler_pitch_roll = self.get_param("flyeagle_a2rl_eav25_bsu_kistler_pitch_roll", "/flyeagle/a2rl/eav25_bsu/kistler_pitch_roll")
        self.flyeagle_a2rl_eav25_bsu_kistler_status = self.get_param("flyeagle_a2rl_eav25_bsu_kistler_status", "/flyeagle/a2rl/eav25_bsu/kistler_status")
        self.flyeagle_a2rl_eav25_bsu_kistler_vel_angle = self.get_param("flyeagle_a2rl_eav25_bsu_kistler_vel_angle", "/flyeagle/a2rl/eav25_bsu/kistler_vel_angle")
        self.flyeagle_a2rl_eav25_bsu_pd_us_status_01 = self.get_param("flyeagle_a2rl_eav25_bsu_pd_us_status_01", "/flyeagle/a2rl/eav25_bsu/pd_us_status_01")
        self.flyeagle_a2rl_eav25_bsu_pit_packet_0 = self.get_param("flyeagle_a2rl_eav25_bsu_pit_packet_0", "/flyeagle/a2rl/eav25_bsu/pit_packet_0")
        self.flyeagle_a2rl_eav25_bsu_pit_packet_1 = self.get_param("flyeagle_a2rl_eav25_bsu_pit_packet_1", "/flyeagle/a2rl/eav25_bsu/pit_packet_1")
        self.flyeagle_a2rl_eav25_bsu_psa_status_01 = self.get_param("flyeagle_a2rl_eav25_bsu_psa_status_01", "/flyeagle/a2rl/eav25_bsu/psa_status_01")
        self.flyeagle_a2rl_eav25_bsu_psa_status_02 = self.get_param("flyeagle_a2rl_eav25_bsu_psa_status_02", "/flyeagle/a2rl/eav25_bsu/psa_status_02")
        self.flyeagle_a2rl_eav25_bsu_rm_status_01 = self.get_param("flyeagle_a2rl_eav25_bsu_rm_status_01", "/flyeagle/a2rl/eav25_bsu/rm_status_01")
        self.flyeagle_a2rl_eav25_bsu_sw_versions_01 = self.get_param("flyeagle_a2rl_eav25_bsu_sw_versions_01", "/flyeagle/a2rl/eav25_bsu/sw_versions_01")
        self.flyeagle_a2rl_eav25_bsu_sw_versions_02 = self.get_param("flyeagle_a2rl_eav25_bsu_sw_versions_02", "/flyeagle/a2rl/eav25_bsu/sw_versions_02")
        self.flyeagle_a2rl_eav25_bsu_sw_versions_03 = self.get_param("flyeagle_a2rl_eav25_bsu_sw_versions_03", "/flyeagle/a2rl/eav25_bsu/sw_versions_03")
        self.flyeagle_a2rl_eav25_bsu_tpms_front = self.get_param("flyeagle_a2rl_eav25_bsu_tpms_front", "/flyeagle/a2rl/eav25_bsu/tpms_front")
        self.flyeagle_a2rl_eav25_bsu_tpms_rear = self.get_param("flyeagle_a2rl_eav25_bsu_tpms_rear", "/flyeagle/a2rl/eav25_bsu/tpms_rear")
        self.flyeagle_a2rl_eav25_bsu_tyre_surface_temp_front = self.get_param("flyeagle_a2rl_eav25_bsu_tyre_surface_temp_front", "/flyeagle/a2rl/eav25_bsu/tyre_surface_temp_front")
        self.flyeagle_a2rl_eav25_bsu_tyre_surface_temp_rear = self.get_param("flyeagle_a2rl_eav25_bsu_tyre_surface_temp_rear", "/flyeagle/a2rl/eav25_bsu/tyre_surface_temp_rear")
        self.flyeagle_a2rl_eav25_bsu_wheels_speed_01 = self.get_param("flyeagle_a2rl_eav25_bsu_wheels_speed_01", "/flyeagle/a2rl/eav25_bsu/wheels_speed_01")

        self.params_help()

        # simulator
        self.a2rl_vn_ins_sub = self.create_subscription(
            VectornavIns, self.a2rl_vn_ins, self.a2rl_vn_ins_callback, 10
        )
        self.clock_sub = self.create_subscription(
            Clock, self.clock, self.clock_callback, 10
        )
        self.flyeagle_ground_truth_sub = self.create_subscription(
            PoseStamped, self.flyeagle_ground_truth, self.flyeagle_ground_truth_callback, 10
        )
        self.vectornav_nav_sat_fix_sub = self.create_subscription(
            NavSatFix, self.vectornav_nav_sat_fix, self.vectornav_nav_sat_fix_callback, 10
        )
        self.vectornav_raw_attitude_sub = self.create_subscription(
            AttitudeGroup, self.vectornav_raw_attitude, self.vectornav_raw_attitude_callback, 10
        )
        self.vectornav_raw_common_sub = self.create_subscription(
            CommonGroup, self.vectornav_raw_common, self.vectornav_raw_common_callback, 10
        )
        self.vectornav_raw_gps_sub = self.create_subscription(
            GpsGroup, self.vectornav_raw_gps, self.vectornav_raw_gps_callback, 10
        )
        self.vectornav_raw_gps2_sub = self.create_subscription(
            GpsGroup, self.vectornav_raw_gps2, self.vectornav_raw_gps2_callback, 10
        )
        self.vectornav_raw_imu_sub = self.create_subscription(
            ImuGroup, self.vectornav_raw_imu, self.vectornav_raw_imu_callback, 10
        )

        # ning
        self.flyeagle_a2rl_observer_ego_loc_sub = self.create_subscription(
            Localization, self.flyeagle_a2rl_observer_ego_loc, self.flyeagle_a2rl_observer_ego_loc_callback, 10
        )
        self.flyeagle_a2rl_observer_ego_state_sub = self.create_subscription(
            EgoState, self.flyeagle_a2rl_observer_ego_state, self.flyeagle_a2rl_observer_ego_state_callback, 10
        )
        self.flyeagle_localization_pose_estimator_pose_from_gps_sub = self.create_subscription(
            PoseWithCovarianceStamped, self.flyeagle_localization_pose_estimator_pose_from_gps, self.flyeagle_localization_pose_estimator_pose_from_gps_callback, 10
        )
        self.flyeagle_localization_twist_estimator_twist_from_ins_sub = self.create_subscription(
            TwistWithCovarianceStamped, self.flyeagle_localization_twist_estimator_twist_from_ins, self.flyeagle_localization_twist_estimator_twist_from_ins_callback, 10
        )
        self.flyeagle_localization_kistler_status_sub = self.create_subscription(
            Bool, self.flyeagle_localization_kistler_status, self.flyeagle_localization_kistler_status_callback, 10
        )
        self.flyeagle_localization_pose_twist_fusion_filter_kinematic_state_sub = self.create_subscription(
            Odometry, self.flyeagle_localization_pose_twist_fusion_filter_kinematic_state, self.flyeagle_localization_pose_twist_fusion_filter_kinematic_state_callback, 10
        )

        # gao
        self.flyeagle_a2rl_eav25_bsu_bad_alive_sub = self.create_subscription(
            Bad_Alive, self.flyeagle_a2rl_eav25_bsu_bad_alive, self.flyeagle_a2rl_eav25_bsu_bad_alive_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_bad_misc_sub = self.create_subscription(
            Bad_Misc, self.flyeagle_a2rl_eav25_bsu_bad_misc, self.flyeagle_a2rl_eav25_bsu_bad_misc_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_bad_powertrain_misc_sub = self.create_subscription(
            Bad_Powertrain_misc, self.flyeagle_a2rl_eav25_bsu_bad_powertrain_misc, self.flyeagle_a2rl_eav25_bsu_bad_powertrain_misc_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_bad_powertrain_press_sub = self.create_subscription(
            Bad_Powertrain_Press, self.flyeagle_a2rl_eav25_bsu_bad_powertrain_press, self.flyeagle_a2rl_eav25_bsu_bad_powertrain_press_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_bad_powertrain_temp_sub = self.create_subscription(
            Bad_Powertrain_Temp, self.flyeagle_a2rl_eav25_bsu_bad_powertrain_temp, self.flyeagle_a2rl_eav25_bsu_bad_powertrain_temp_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_bad_ride_front_sub = self.create_subscription(
            Bad_Ride_Front, self.flyeagle_a2rl_eav25_bsu_bad_ride_front, self.flyeagle_a2rl_eav25_bsu_bad_ride_front_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_bad_ride_rear_sub = self.create_subscription(
            Bad_Ride_Rear, self.flyeagle_a2rl_eav25_bsu_bad_ride_rear, self.flyeagle_a2rl_eav25_bsu_bad_ride_rear_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_bad_wheel_load_sub = self.create_subscription(
            Bad_Wheel_Load, self.flyeagle_a2rl_eav25_bsu_bad_wheel_load, self.flyeagle_a2rl_eav25_bsu_bad_wheel_load_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_bad_z_accel_body_sub = self.create_subscription(
            Bad_Z_Accel_Body, self.flyeagle_a2rl_eav25_bsu_bad_z_accel_body, self.flyeagle_a2rl_eav25_bsu_bad_z_accel_body_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_bosch_pitch_sub = self.create_subscription(
            Bosch_Pitch, self.flyeagle_a2rl_eav25_bsu_bosch_pitch, self.flyeagle_a2rl_eav25_bsu_bosch_pitch_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_bosch_roll_sub = self.create_subscription(
            Bosch_Roll, self.flyeagle_a2rl_eav25_bsu_bosch_roll, self.flyeagle_a2rl_eav25_bsu_bosch_roll_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_bosch_yaw_sub = self.create_subscription(
            Bosch_Yaw, self.flyeagle_a2rl_eav25_bsu_bosch_yaw, self.flyeagle_a2rl_eav25_bsu_bosch_yaw_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_brake_disk_temp_sub = self.create_subscription(
            Brake_Disk_Temp, self.flyeagle_a2rl_eav25_bsu_brake_disk_temp, self.flyeagle_a2rl_eav25_bsu_brake_disk_temp_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_bsu_ranges_01_sub = self.create_subscription(
            BSU_Ranges_01, self.flyeagle_a2rl_eav25_bsu_bsu_ranges_01, self.flyeagle_a2rl_eav25_bsu_bsu_ranges_01_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_bsu_ranges_02_sub = self.create_subscription(
            BSU_Ranges_02, self.flyeagle_a2rl_eav25_bsu_bsu_ranges_02, self.flyeagle_a2rl_eav25_bsu_bsu_ranges_02_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_bsu_status_01_sub = self.create_subscription(
            BSU_Status_01, self.flyeagle_a2rl_eav25_bsu_bsu_status_01, self.flyeagle_a2rl_eav25_bsu_bsu_status_01_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_cba_status_fl_sub = self.create_subscription(
            CBA_Status_FL, self.flyeagle_a2rl_eav25_bsu_cba_status_fl, self.flyeagle_a2rl_eav25_bsu_cba_status_fl_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_cba_status_fr_sub = self.create_subscription(
            CBA_Status_FR, self.flyeagle_a2rl_eav25_bsu_cba_status_fr, self.flyeagle_a2rl_eav25_bsu_cba_status_fr_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_cba_status_rl_sub = self.create_subscription(
            CBA_Status_RL, self.flyeagle_a2rl_eav25_bsu_cba_status_rl, self.flyeagle_a2rl_eav25_bsu_cba_status_rl_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_cba_status_rr_sub = self.create_subscription(
            CBA_Status_RR, self.flyeagle_a2rl_eav25_bsu_cba_status_rr, self.flyeagle_a2rl_eav25_bsu_cba_status_rr_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_diagnostic_word_01_sub = self.create_subscription(
            DiagnosticWord_01, self.flyeagle_a2rl_eav25_bsu_diagnostic_word_01, self.flyeagle_a2rl_eav25_bsu_diagnostic_word_01_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_diagnostic_word_02_sub = self.create_subscription(
            DiagnosticWord_02, self.flyeagle_a2rl_eav25_bsu_diagnostic_word_02, self.flyeagle_a2rl_eav25_bsu_diagnostic_word_02_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_em_status_01_sub = self.create_subscription(
            EM_Status_01, self.flyeagle_a2rl_eav25_bsu_em_status_01, self.flyeagle_a2rl_eav25_bsu_em_status_01_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_hl_msg_01_sub = self.create_subscription(
            HL_Msg_01, self.flyeagle_a2rl_eav25_bsu_hl_msg_01, self.flyeagle_a2rl_eav25_bsu_hl_msg_01_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_hl_msg_02_sub = self.create_subscription(
            HL_Msg_02, self.flyeagle_a2rl_eav25_bsu_hl_msg_02, self.flyeagle_a2rl_eav25_bsu_hl_msg_02_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_hl_msg_03_sub = self.create_subscription(
            HL_Msg_03, self.flyeagle_a2rl_eav25_bsu_hl_msg_03, self.flyeagle_a2rl_eav25_bsu_hl_msg_03_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_hl_msg_04_sub = self.create_subscription(
            HL_Msg_04, self.flyeagle_a2rl_eav25_bsu_hl_msg_04, self.flyeagle_a2rl_eav25_bsu_hl_msg_04_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_hl_msg_05_sub = self.create_subscription(
            HL_Msg_05, self.flyeagle_a2rl_eav25_bsu_hl_msg_05, self.flyeagle_a2rl_eav25_bsu_hl_msg_05_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_hl_msg_06_sub = self.create_subscription(
            HL_Msg_06, self.flyeagle_a2rl_eav25_bsu_hl_msg_06, self.flyeagle_a2rl_eav25_bsu_hl_msg_06_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_ice_status_01_sub = self.create_subscription(
            ICE_Status_01, self.flyeagle_a2rl_eav25_bsu_ice_status_01, self.flyeagle_a2rl_eav25_bsu_ice_status_01_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_ice_status_02_sub = self.create_subscription(
            ICE_Status_02, self.flyeagle_a2rl_eav25_bsu_ice_status_02, self.flyeagle_a2rl_eav25_bsu_ice_status_02_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_kistler_acc_body_sub = self.create_subscription(
            Kistler_Acc_Body, self.flyeagle_a2rl_eav25_bsu_kistler_acc_body, self.flyeagle_a2rl_eav25_bsu_kistler_acc_body_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_kistler_angle_vel_body_sub = self.create_subscription(
            Kistler_Angle_Vel_Body, self.flyeagle_a2rl_eav25_bsu_kistler_angle_vel_body, self.flyeagle_a2rl_eav25_bsu_kistler_angle_vel_body_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_kistler_correvit_sub = self.create_subscription(
            Kistler_Correvit, self.flyeagle_a2rl_eav25_bsu_kistler_correvit, self.flyeagle_a2rl_eav25_bsu_kistler_correvit_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_kistler_distance_sub = self.create_subscription(
            Kistler_Distance, self.flyeagle_a2rl_eav25_bsu_kistler_distance, self.flyeagle_a2rl_eav25_bsu_kistler_distance_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_kistler_pitch_roll_sub = self.create_subscription(
            Kistler_Pitch_Roll, self.flyeagle_a2rl_eav25_bsu_kistler_pitch_roll, self.flyeagle_a2rl_eav25_bsu_kistler_pitch_roll_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_kistler_status_sub = self.create_subscription(
            Kistler_Status, self.flyeagle_a2rl_eav25_bsu_kistler_status, self.flyeagle_a2rl_eav25_bsu_kistler_status_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_kistler_vel_angle_sub = self.create_subscription(
            Kistler_Vel_Angle, self.flyeagle_a2rl_eav25_bsu_kistler_vel_angle, self.flyeagle_a2rl_eav25_bsu_kistler_vel_angle_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_pd_us_status_01_sub = self.create_subscription(
            PDUs_Status_01, self.flyeagle_a2rl_eav25_bsu_pd_us_status_01, self.flyeagle_a2rl_eav25_bsu_pd_us_status_01_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_pit_packet_0_sub = self.create_subscription(
            PIT_Packet_0, self.flyeagle_a2rl_eav25_bsu_pit_packet_0, self.flyeagle_a2rl_eav25_bsu_pit_packet_0_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_pit_packet_1_sub = self.create_subscription(
            PIT_Packet_1, self.flyeagle_a2rl_eav25_bsu_pit_packet_1, self.flyeagle_a2rl_eav25_bsu_pit_packet_1_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_psa_status_01_sub = self.create_subscription(
            PSA_Status_01, self.flyeagle_a2rl_eav25_bsu_psa_status_01, self.flyeagle_a2rl_eav25_bsu_psa_status_01_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_psa_status_02_sub = self.create_subscription(
            PSA_Status_02, self.flyeagle_a2rl_eav25_bsu_psa_status_02, self.flyeagle_a2rl_eav25_bsu_psa_status_02_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_rm_status_01_sub = self.create_subscription(
            RM_Status_01, self.flyeagle_a2rl_eav25_bsu_rm_status_01, self.flyeagle_a2rl_eav25_bsu_rm_status_01_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_sw_versions_01_sub = self.create_subscription(
            SW_Versions_01, self.flyeagle_a2rl_eav25_bsu_sw_versions_01, self.flyeagle_a2rl_eav25_bsu_sw_versions_01_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_sw_versions_02_sub = self.create_subscription(
            SW_Versions_02, self.flyeagle_a2rl_eav25_bsu_sw_versions_02, self.flyeagle_a2rl_eav25_bsu_sw_versions_02_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_sw_versions_03_sub = self.create_subscription(
            SW_Versions_03, self.flyeagle_a2rl_eav25_bsu_sw_versions_03, self.flyeagle_a2rl_eav25_bsu_sw_versions_03_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_tpms_front_sub = self.create_subscription(
            Tpms_Front, self.flyeagle_a2rl_eav25_bsu_tpms_front, self.flyeagle_a2rl_eav25_bsu_tpms_front_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_tpms_rear_sub = self.create_subscription(
            Tpms_Rear, self.flyeagle_a2rl_eav25_bsu_tpms_rear, self.flyeagle_a2rl_eav25_bsu_tpms_rear_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_tyre_surface_temp_front_sub = self.create_subscription(
            Tyre_Surface_Temp_Front, self.flyeagle_a2rl_eav25_bsu_tyre_surface_temp_front, self.flyeagle_a2rl_eav25_bsu_tyre_surface_temp_front_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_tyre_surface_temp_rear_sub = self.create_subscription(
            Tyre_Surface_Temp_Rear, self.flyeagle_a2rl_eav25_bsu_tyre_surface_temp_rear, self.flyeagle_a2rl_eav25_bsu_tyre_surface_temp_rear_callback, 10
        )
        self.flyeagle_a2rl_eav25_bsu_wheels_speed_01_sub = self.create_subscription(
            Wheels_Speed_01, self.flyeagle_a2rl_eav25_bsu_wheels_speed_01, self.flyeagle_a2rl_eav25_bsu_wheels_speed_01_callback, 10
        )

        ## simulator
        self.sms_a2rl_vn_ins_pub = Publisher(self.a2rl_vn_ins, "nav_msgs::VectornavIns")
        self.sms_clock_pub = Publisher(self.clock, "std_msgs::Clock")
        self.sms_flyeagle_ground_truth_pub = Publisher(self.flyeagle_ground_truth, "geometry_msgs::PoseInFrame")
        self.sms_vectornav_nav_sat_fix_pub = Publisher(self.vectornav_nav_sat_fix, "sensor_msgs::NavSatFix")
        self.sms_vectornav_raw_attitude_pub = Publisher(self.vectornav_raw_attitude, "nav_msgs::VectornavAttitudeGroup")
        self.sms_vectornav_raw_common_pub = Publisher(self.vectornav_raw_common, "nav_msgs::VectornavCommonGroup")
        self.sms_vectornav_raw_gps_pub = Publisher(self.vectornav_raw_gps, "nav_msgs::VectornavGpsGroup")
        self.sms_vectornav_raw_gps2_pub = Publisher(self.vectornav_raw_gps2, "nav_msgs::VectornavGpsGroup")
        self.sms_vectornav_raw_imu_pub = Publisher(self.vectornav_raw_imu, "nav_msgs::VectornavImuGroup")

        ## ning
        self.sms_flyeagle_a2rl_observer_ego_loc_pub = Publisher(self.flyeagle_a2rl_observer_ego_loc, "nav_msgs::Localization")
        self.sms_flyeagle_a2rl_observer_ego_state_pub = Publisher(self.flyeagle_a2rl_observer_ego_state, "vehicle_msgs::EgoState")
        self.sms_flyeagle_localization_pose_estimator_pose_from_gps_pub = Publisher(self.flyeagle_localization_pose_estimator_pose_from_gps, "geometry_msgs::PoseWithCovariance")
        self.sms_flyeagle_localization_twist_estimator_twist_from_ins_pub = Publisher(self.flyeagle_localization_twist_estimator_twist_from_ins, "geometry_msgs::TwistWithCovariance")
        self.sms_flyeagle_localization_kistler_status_pub = Publisher(self.flyeagle_localization_kistler_status, "std_msgs::Boolean")
        # self.sms_flyeagle_a2rl_eav24_bsu_kistler_correvit_pub = Publisher(self.flyeagle_a2rl_eav24_bsu_kistler_correvit, "std_msgs::Null")
        self.sms_flyeagle_localization_pose_twist_fusion_filter_kinematic_state_pub = Publisher(self.flyeagle_localization_pose_twist_fusion_filter_kinematic_state, "nav_msgs::Odometry")

        ## gao
        self.sms_flyeagle_a2rl_eav25_bsu_bad_alive_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bad_alive, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_bad_misc_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bad_misc, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_bad_powertrain_misc_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bad_powertrain_misc, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_bad_powertrain_press_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bad_powertrain_press, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_bad_powertrain_temp_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bad_powertrain_temp, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_bad_ride_front_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bad_ride_front, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_bad_ride_rear_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bad_ride_rear, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_bad_wheel_load_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bad_wheel_load, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_bad_z_accel_body_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bad_z_accel_body, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_bosch_pitch_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bosch_pitch, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_bosch_roll_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bosch_roll, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_bosch_yaw_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bosch_yaw, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_brake_disk_temp_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_brake_disk_temp, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_bsu_ranges_01_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bsu_ranges_01, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_bsu_ranges_02_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bsu_ranges_02, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_bsu_status_01_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_bsu_status_01, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_cba_status_fl_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_cba_status_fl, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_cba_status_fr_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_cba_status_fr, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_cba_status_rl_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_cba_status_rl, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_cba_status_rr_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_cba_status_rr, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_diagnostic_word_01_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_diagnostic_word_01, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_diagnostic_word_02_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_diagnostic_word_02, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_em_status_01_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_em_status_01, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_hl_msg_01_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_hl_msg_01, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_hl_msg_02_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_hl_msg_02, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_hl_msg_03_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_hl_msg_03, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_hl_msg_04_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_hl_msg_04, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_hl_msg_05_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_hl_msg_05, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_hl_msg_06_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_hl_msg_06, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_ice_status_01_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_ice_status_01, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_ice_status_02_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_ice_status_02, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_kistler_acc_body_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_kistler_acc_body, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_kistler_angle_vel_body_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_kistler_angle_vel_body, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_kistler_correvit_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_kistler_correvit, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_kistler_distance_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_kistler_distance, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_kistler_pitch_roll_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_kistler_pitch_roll, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_kistler_status_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_kistler_status, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_kistler_vel_angle_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_kistler_vel_angle, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_pd_us_status_01_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_pd_us_status_01, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_pit_packet_0_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_pit_packet_0, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_pit_packet_1_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_pit_packet_1, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_psa_status_01_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_psa_status_01, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_psa_status_02_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_psa_status_02, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_rm_status_01_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_rm_status_01, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_sw_versions_01_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_sw_versions_01, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_sw_versions_02_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_sw_versions_02, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_sw_versions_03_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_sw_versions_03, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_tpms_front_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_tpms_front, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_tpms_rear_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_tpms_rear, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_tyre_surface_temp_front_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_tyre_surface_temp_front, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_tyre_surface_temp_rear_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_tyre_surface_temp_rear, "std_msgs::Null")
        self.sms_flyeagle_a2rl_eav25_bsu_wheels_speed_01_pub = Publisher(self.flyeagle_a2rl_eav25_bsu_wheels_speed_01, "std_msgs::Null")

    def a2rl_vn_ins_callback(self, msg):
        if msg is not None:
            sms_msg = def_msg('nav_msgs::VectornavIns')
            sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
            sms_msg['time_gps'] = msg.time_gps.nanoseconds / 1e9
            sms_msg['ins_status'] = msg.ins_status 
            sms_msg['acceleration_ins'] = {
                'x': msg.acceleration_ins.x, 'y': msg.acceleration_ins.y, 'z': msg.acceleration_ins.z 
            }
            sms_msg['angular_rate_ins'] = {
                'x': msg.angular_rate_ins.x,
                'y': msg.angular_rate_ins.y,
                'z': msg.angular_rate_ins.z 
            }
            sms_msg['position_enu_ins'] = {
                'x': msg.position_enu_ins.x,
                'y': msg.position_enu_ins.y,
                'z': msg.position_enu_ins.z 
            }
            sms_msg['velocity_enu_ins'] = {
                'x': msg.velocity_enu_ins.x,
                'y': msg.velocity_enu_ins.y,
                'z': msg.velocity_enu_ins.z 
            }
            sms_msg['velocity_body_ins'] = {
                'x': msg.velocity_body_ins.x,
                'y': msg.velocity_body_ins.y,
                'z': msg.velocity_body_ins.z 
            }
            sms_msg['pos_stddev_ins'] = msg.pos_stddev_ins 
            sms_msg['vel_stddev_ins'] = msg.vel_stddev_ins 
            sms_msg['orientation_ypr'] = {
                'x': msg.orientation_ypr.x,
                'y': msg.orientation_ypr.y,
                'z': msg.orientation_ypr.z 
            }
            sms_msg['orientation_stddev'] = {
                'x': msg.orientation_stddev.x,
                'y': msg.orientation_stddev.y,
                'z': msg.orientation_stddev.z 
            }

            self.sms_a2rl_vn_ins_pub.publish(sms_msg)
        else:
            self.get_logger().warning("Received None message for /a2rl/vn/ins")
    
    def clock_callback(self, msg):
        timestamp = msg.clock.sec + msg.clock.nanosec / 1e9
        sms_msg = def_msg('std_msgs::Clock')
        sms_msg['timestamp'] = timestamp

        self.sms_clock_pub.publish(sms_msg)

    def flyeagle_ground_truth_callback(self, msg):
        if msg is not None:
            ros_time = msg.header.stamp
            timestamp = ros_time.sec + ros_time.nanosec / 1e9
            sms_msg = def_msg('geometry_msgs::PoseInFrame')
            sms_msg['timestamp'] = timestamp
            sms_msg['pose'] = def_msg('geometry_msgs::Pose')
            sms_msg['pose']['position'] = [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]
            sms_msg['pose']['orientation'] = [msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w]

            self.sms_flyeagle_ground_truth_pub.publish(sms_msg)
    
    def vectornav_nav_sat_fix_callback(self, msg):
        ros_time = msg.header.stamp
        sms_msg = def_msg('sensor_msgs::NavSatFix')
        sms_msg['timestamp'] = ros_time.sec + ros_time.nanosec / 1e9
        sms_msg['status'] = [msg.status.status, msg.status.service]
        sms_msg['latitude'] = msg.latitude
        sms_msg['longitude'] = msg.longitude
        sms_msg['altitude'] = msg.altitude
        sms_msg['position_covariance'] = list(msg.position_covariance[:9])
        sms_msg['position_covariance_type'] = msg.position_covariance_type
        
        self.sms_vectornav_nav_sat_fix_pub.publish(sms_msg)

    def vectornav_raw_attitude_callback(self, msg):
        ros_time = msg.header.stamp
        sms_msg = def_msg('nav_msgs::VectornavAttitudeGroup')
        sms_msg['timestamp'] = ros_time.sec + ros_time.nanosec / 1e9
        sms_msg['frame_id'] = msg.header.frame_id
        sms_msg['group_fields'] = msg.group_fields
        sms_msg['vpestatus'] = {
            'attitude_quality': msg.vpestatus.attitude_quality,
            'gyro_saturation': msg.vpestatus.gyro_saturation,
            'gyro_saturation_recovery': msg.vpestatus.gyro_saturation_recovery,
            'mag_disturbance': msg.vpestatus.mag_disturbance,
            'mag_saturation': msg.vpestatus.mag_saturation,
            'acc_disturbance': msg.vpestatus.acc_disturbance,
            'acc_saturation': msg.vpestatus.acc_saturation,
            'known_mag_disturbance': msg.vpestatus.known_mag_disturbance,
            'known_accel_disturbance': msg.vpestatus.known_accel_disturbance
        }
        sms_msg['yawpitchroll'] = {
            'x': msg.yawpitchroll.x,
            'y': msg.yawpitchroll.y,
            'z': msg.yawpitchroll.z
        }
        sms_msg['quaternion'] = {
            'x': msg.quaternion.x,
            'y': msg.quaternion.y,
            'z': msg.quaternion.z,
            'w': msg.quaternion.w
        }
        sms_msg['dcm'] = list(msg.dcm)
        sms_msg['magned'] = {
            'x': msg.magned.x,
            'y': msg.magned.y,
            'z': msg.magned.z
        }
        sms_msg['accelned'] = {
            'x': msg.accelned.x,
            'y': msg.accelned.y,
            'z': msg.accelned.z
        }
        sms_msg['linearaccelbody'] = {
            'x': msg.linearaccelbody.x,
            'y': msg.linearaccelbody.y,
            'z': msg.linearaccelbody.z
        }
        sms_msg['linearaccelned'] = {
            'x': msg.linearaccelned.x, 'y': msg.linearaccelned.y, 'z': msg.linearaccelned.z
        }
        sms_msg['ypru'] = {
            'x': msg.ypru.x, 'y': msg.ypru.y, 'z': msg.ypru.z
        }

        self.sms_vectornav_raw_attitude_pub.publish(sms_msg)

    def vectornav_raw_common_callback(self, msg):
        ros_time = msg.header.stamp
        sms_msg = def_msg('nav_msgs::VectornavCommonGroup')
        
        sms_msg['timestamp'] = ros_time.sec + ros_time.nanosec / 1e9
        sms_msg['frame_id'] = msg.header.frame_id
        
        sms_msg['group_fields'] = msg.group_fields
        
        sms_msg['timestartup'] = msg.timestartup
        sms_msg['timegps'] = msg.timegps
        sms_msg['timesyncin'] = msg.timesyncin
        
        sms_msg['yawpitchroll'] = {
            'x': msg.yawpitchroll.x, 'y': msg.yawpitchroll.y, 'z': msg.yawpitchroll.z
        }
        sms_msg['quaternion'] = {
            'x': msg.quaternion.x,
            'y': msg.quaternion.y,
            'z': msg.quaternion.z,
            'w': msg.quaternion.w
        }
        sms_msg['angularrate'] = {
            'x': msg.angularrate.x, 'y': msg.angularrate.y, 'z': msg.angularrate.z
        }
        sms_msg['position'] = {
            'x': msg.position.x, 'y': msg.position.y, 'z': msg.position.z
        }
        sms_msg['velocity'] = {
            'x': msg.velocity.x, 'y': msg.velocity.y, 'z': msg.velocity.z
        } 
        sms_msg['accel'] = {
            'x': msg.accel.x, 'y': msg.accel.y, 'z': msg.accel.z
        }
        sms_msg['imu_accel'] = {
            'x': msg.imu_accel.x, 'y': msg.imu_accel.y, 'z': msg.imu_accel.z
        }
        sms_msg['imu_rate'] = {
            'x': msg.imu_rate.x, 'y': msg.imu_rate.y, 'z': msg.imu_rate.z
        }
        sms_msg['magpres_mag'] = {
            'x': msg.magpres_mag.x, 'y': msg.magpres_mag.y, 'z': msg.magpres_mag.z
        }
        sms_msg['magpres_temp'] = msg.magpres_temp
        sms_msg['magpres_pres'] = msg.magpres_pres
        sms_msg['deltatheta_dtime'] = msg.deltatheta_dtime
        sms_msg['deltatheta_dtheta'] = {
            'x': msg.deltatheta_dtheta.x, 'y': msg.deltatheta_dtheta.y, 'z': msg.deltatheta_dtheta.z
        }
        sms_msg['deltatheta_dvel'] = {
            'x': msg.deltatheta_dvel.x, 'y': msg.deltatheta_dvel.y, 'z': msg.deltatheta_dvel.z
        }
        sms_msg['insstatus'] = {
            'mode': msg.insstatus.mode,
            'gps_fix': msg.insstatus.gps_fix,
            'time_error': msg.insstatus.time_error,
            'imu_error': msg.insstatus.imu_error,
            'mag_pres_error': msg.insstatus.mag_pres_error,
            'gps_error': msg.insstatus.gps_error,
            'gps_heading_ins': msg.insstatus.gps_heading_ins,
            'gps_compass': msg.insstatus.gps_compass
        }

        sms_msg['syncincnt'] = msg.syncincnt
        sms_msg['timegpspps'] = msg.timegpspps

        self.sms_vectornav_raw_common_pub.publish(sms_msg)
    
    def vectornav_raw_gps_callback(self, msg):
        ros_time = msg.header.stamp
        sms_msg = def_msg('nav_msgs::VectornavGpsGroup')
        
        sms_msg['timestamp'] = ros_time.sec + ros_time.nanosec / 1e9
        sms_msg['frame_id'] = msg.header.frame_id

        sms_msg['group_fields'] = msg.group_fields

        sms_msg['utc'] = {
            'year': msg.utc.year,
            'month': msg.utc.month,
            'day': msg.utc.day,
            'hour': msg.utc.hour,
            'min': msg.utc.min,
            'sec': msg.utc.sec,
            'ms': msg.utc.ms
        }
        sms_msg['tow'] = msg.tow  # Time of Week
        sms_msg['week'] = msg.week
        sms_msg['numsats'] = msg.numsats
        sms_msg['fix'] = msg.fix
        sms_msg['poslla'] = {
            'x': msg.poslla.x, 'y': msg.poslla.y, 'z': msg.poslla.z
        }
        sms_msg['posecef'] = {
            'x': msg.posecef.x, 'y': msg.posecef.y, 'z': msg.posecef.z
        }
        sms_msg['velned'] = {
            'x': msg.velned.x, 'y': msg.velned.y, 'z': msg.velned.z
        }
        sms_msg['velecef'] = {
            'x': msg.velecef.x, 'y': msg.velecef.y, 'z': msg.velecef.z
        }
        sms_msg['posu'] = {
            'x': msg.posu.x, 'y': msg.posu.y, 'z': msg.posu.z
        }
        sms_msg['velu'] = msg.velu
        sms_msg['timeu'] = msg.timeu
        sms_msg['timeinfo'] = {
            'status': msg.timeinfo_status,
            'leapseconds': msg.timeinfo_leapseconds
        }
        sms_msg['dop'] = {
            'g': msg.dop.g,  # GDOP
            'p': msg.dop.p,  # PDOP
            't': msg.dop.t,  # TDOP
            'v': msg.dop.v,  # VDOP
            'h': msg.dop.h,  # HDOP
            'n': msg.dop.n,  # NDOP
            'e': msg.dop.e   # EDOP
        }

        self.sms_vectornav_raw_gps_pub.publish(sms_msg)
    
    def vectornav_raw_gps2_callback(self, msg):
        ros_time = msg.header.stamp
        sms_msg = def_msg('nav_msgs::VectornavGpsGroup')
        
        sms_msg['timestamp'] = ros_time.sec + ros_time.nanosec / 1e9
        sms_msg['frame_id'] = msg.header.frame_id

        sms_msg['group_fields'] = msg.group_fields

        sms_msg['utc'] = {
            'year': msg.utc.year,
            'month': msg.utc.month,
            'day': msg.utc.day,
            'hour': msg.utc.hour,
            'min': msg.utc.min,
            'sec': msg.utc.sec,
            'ms': msg.utc.ms
        }
        sms_msg['tow'] = msg.tow  # Time of Week
        sms_msg['week'] = msg.week
        sms_msg['numsats'] = msg.numsats
        sms_msg['fix'] = msg.fix
        sms_msg['poslla'] = {
            'x': msg.poslla.x, 'y': msg.poslla.y, 'z': msg.poslla.z
        }
        sms_msg['posecef'] = {
            'x': msg.posecef.x, 'y': msg.posecef.y, 'z': msg.posecef.z
        }
        sms_msg['velned'] = {
            'x': msg.velned.x, 'y': msg.velned.y, 'z': msg.velned.z
        }
        sms_msg['velecef'] = {
            'x': msg.velecef.x, 'y': msg.velecef.y, 'z': msg.velecef.z
        }
        sms_msg['posu'] = {
            'x': msg.posu.x, 'y': msg.posu.y, 'z': msg.posu.z
        }
        sms_msg['velu'] = msg.velu
        sms_msg['timeu'] = msg.timeu
        sms_msg['timeinfo'] = {
            'status': msg.timeinfo_status,
            'leapseconds': msg.timeinfo_leapseconds
        }
        sms_msg['dop'] = {
            'g': msg.dop.g,  # GDOP
            'p': msg.dop.p,  # PDOP
            't': msg.dop.t,  # TDOP
            'v': msg.dop.v,  # VDOP
            'h': msg.dop.h,  # HDOP
            'n': msg.dop.n,  # NDOP
            'e': msg.dop.e   # EDOP
        }

        self.sms_vectornav_raw_gps2_pub.publish(sms_msg)

    def vectornav_raw_imu_callback(self, msg):
        ros_time = msg.header.stamp
        sms_msg = def_msg('nav_msgs::VectornavImuGroup')

        sms_msg['timestamp'] = ros_time.sec + ros_time.nanosec / 1e9
        sms_msg['frame_id'] = msg.header.frame_id
        sms_msg['group_fields'] = msg.group_fields
        sms_msg['imustatus'] = msg.imustatus
        sms_msg['uncompmag'] = {
            'x': msg.uncompmag.x, 'y': msg.uncompmag.y, 'z': msg.uncompmag.z
        }
        sms_msg['uncompaccel'] = {
            'x': msg.uncompaccel.x, 'y': msg.uncompaccel.y, 'z': msg.uncompaccel.z
        }
        sms_msg['uncompgyro'] = {
            'x': msg.uncompgyro.x,
            'y': msg.uncompgyro.y,
            'z': msg.uncompgyro.z
        }
        sms_msg['temp'] = msg.temp
        sms_msg['pres'] = msg.pres
        sms_msg['deltatheta_time'] = msg.deltatheta_time
        sms_msg['deltatheta_dtheta'] = {
            'x': msg.deltatheta_dtheta.x,
            'y': msg.deltatheta_dtheta.y,
            'z': msg.deltatheta_dtheta.z
        }
        sms_msg['deltavel'] = {
            'x': msg.deltavel.x, 'y': msg.deltavel.y, 'z': msg.deltavel.z
        }
        sms_msg['mag'] = {
            'x': msg.mag.x, 'y': msg.mag.y, 'z': msg.mag.z
        }
        sms_msg['accel'] = {
            'x': msg.accel.x, 'y': msg.accel.y, 'z': msg.accel.z
        }
        sms_msg['angularrate'] = {
            'x': msg.angularrate.x, 'y': msg.angularrate.y, 'z': msg.angularrate.z
        }
        sms_msg['sensat'] = msg.sensat

        self.sms_vectornav_raw_imu_pub.publish(sms_msg)
           
    def flyeagle_a2rl_observer_ego_loc_callback(self, msg):
        sms_msg = def_msg('nav_msgs::Localization')
        sms_msg['timestamp'] = msg.timestamp.nanoseconds / 1e9
        sms_msg['position'] = [msg.position.x, msg.position.y, msg.position.z]
        sms_msg['position_stddev'] = [msg.position_stddev.x, msg.position_stddev.y, msg.position_stddev.z]
        sms_msg['orientation_ypr'] = [msg.orientation_ypr.x, msg.orientation_ypr.y, msg.orientation_ypr.z]
        sms_msg['orientation_stddev'] = [msg.orientation_stddev.x, msg.orientation_stddev.y, msg.orientation_stddev.z]

        self.sms_flyeagle_a2rl_observer_ego_loc_pub.publish(sms_msg)

    def flyeagle_a2rl_observer_ego_state_callback(self, msg):
        sms_msg = def_msg('vehicle_msgs::EgoState')
        sms_msg['timestamp'] = msg.timestamp.nanoseconds / 1e9
        sms_msg['velocity'] = [msg.velocity.x, msg.velocity.y, msg.velocity.z]   
        sms_msg['velocity_stddev'] = [msg.velocity_stddev.x, msg.velocity_stddev.y, msg.velocity_stddev.z] 
        sms_msg['angular_rate'] = [msg.angular_rate.x, msg.angular_rate.y, msg.angular_rate.z]   
        sms_msg['acceleration'] = [msg.acceleration.x, msg.acceleration.y, msg.acceleration.z]  
        sms_msg['acceleration_stddev'] = [msg.acceleration_stddev.x, msg.acceleration_stddev.y, msg.acceleration_stddev.z]
        sms_msg['wheels_speed'] = [msg.wheels_speed.fl, msg.wheels_speed.fr, msg.wheels_speed.rl, msg.wheels_speed.rr]
        sms_msg['wheels_toe_angle'] = [msg.wheels_toe_angle.fl, msg.wheels_toe_angle.fr, msg.wheels_toe_angle.rl, msg.wheels_toe_angle.rr]
        sms_msg['ypr_chassis'] = [msg.ypr_chassis.x, msg.ypr_chassis.y, msg.ypr_chassis.z] 

        self.sms_flyeagle_a2rl_observer_ego_state_pub.publish(sms_msg)
    
    def flyeagle_localization_pose_estimator_pose_from_gps_callback(self, msg):
        ros_time = msg.header.stamp
        timestamp = ros_time.sec + ros_time.nanosec / 1e9
        sms_msg = def_msg('geometry_msgs::PoseWithCovariance')
        sms_msg['timestamp'] = timestamp
        sms_msg['frame_id'] = msg.header.frame_id
        sms_msg['position'] = [msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]
        sms_msg['orientation'] = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]
        sms_msg['pose_covariance'] = msg.pose.covariance

        self.sms_flyeagle_localization_pose_estimator_pose_from_gps_pub.publish(sms_msg)
    
    def flyeagle_localization_twist_estimator_twist_from_ins_callback(self, msg):
        ros_time = msg.header.stamp
        timestamp = ros_time.sec + ros_time.nanosec / 1e9
        sms_msg = def_msg('geometry_msgs::TwistWithCovariance')
        sms_msg['timestamp'] = timestamp
        sms_msg['frame_id'] = msg.header.frame_id
        sms_msg['linear'] = [msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]
        sms_msg['angular'] = [msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z]
        sms_msg['twist_covariance'] = msg.twist.covariance

        self.sms_flyeagle_localization_twist_estimator_twist_from_ins_pub.publish(sms_msg)

    def flyeagle_localization_kistler_status_callback(self, msg):
        sms_msg = def_msg('std_msgs::Boolean')
        sms_msg['data'] = msg.data

        self.sms_flyeagle_localization_kistler_status_pub.publish(sms_msg)
    
    # def flyeagle_a2rl_eav24_bsu_kistler_correvit_callback(self, msg):
    #     sms_msg = def_msg('std_msgs::Null')
    #     sms_msg['timestamp'] = msg.timestamp.nanoseconds / 1e9
    #     sms_msg['vel_x_cor'] = msg.vel_x_cor
    #     sms_msg['vel_y_cor'] = msg.vel_y_cor
    #     sms_msg['vel_cor'] = msg.vel_cor
    #     sms_msg['angle_cor'] = msg.angle_cor

    #     self.sms_flyeagle_a2rl_eav24_bsu_kistler_correvit_pub.publish(sms_msg)
    
    def flyeagle_localization_pose_twist_fusion_filter_kinematic_state_callback(self, msg):
        ros_time = msg.header.stamp
        timestamp = ros_time.sec + ros_time.nanosec / 1e9
        sms_msg = def_msg('nav_msgs::Odometry')
        sms_msg['timestamp'] = timestamp
        sms_msg['frame_id'] = msg.header.frame_id
        sms_msg['child_frame_id'] = msg.child_frame_id
        sms_msg['position'] = [msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]
        sms_msg['orientation'] = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]
        sms_msg['pose_covariance'] = msg.pose.covariance
        sms_msg['linear'] = [msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]
        sms_msg['angular'] = [msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z]
        sms_msg['twist_covariance'] = msg.twist.twist.covariance

        self.sms_flyeagle_localization_pose_twist_fusion_filter_kinematic_state_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_bad_alive_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['bad_alive_badenia'] = msg.bad_alive_badenia

        self.sms_flyeagle_a2rl_eav25_bsu_bad_alive_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_bad_misc_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['bad_lap_time'] = msg.bad_lap_time
        sms_msg['bad_lap_distance'] = msg.bad_lap_distance
        sms_msg['bad_lap_number'] = msg.bad_lap_number
        sms_msg['battery_voltage'] = msg.battery_voltage

        self.sms_flyeagle_a2rl_eav25_bsu_bad_misc_pub.publish(sms_msg)
    
    def flyeagle_a2rl_eav25_bsu_bad_powertrain_misc_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['bad_m1_throttle_position'] = msg.bad_m1_throttle_position
        sms_msg['bad_m1_engine_speed_limit'] = msg.bad_m1_engine_speed_limit
        sms_msg['bad_fuel_used_mass'] = msg.bad_fuel_used_mass

        self.sms_flyeagle_a2rl_eav25_bsu_bad_powertrain_misc_pub.publish(sms_msg)
    
    def flyeagle_a2rl_eav25_bsu_bad_powertrain_press_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['bad_m1_engine_oil_press'] = msg.bad_m1_engine_oil_press
        sms_msg['bad_m1_coolant_press'] = msg.bad_m1_coolant_press
        sms_msg['bad_m1_fuel_press_direct'] = msg.bad_m1_fuel_press_direct
        sms_msg['bad_m1_boost_press'] = msg.bad_m1_boost_press

        self.sms_flyeagle_a2rl_eav25_bsu_bad_powertrain_press_pub.publish(sms_msg)
    
    def flyeagle_a2rl_eav25_bsu_bad_powertrain_temp_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['bad_m1_engine_oil_temp'] = msg.bad_m1_engine_oil_temp
        sms_msg['bad_m1_coolant_temp'] = msg.bad_m1_coolant_temp
        sms_msg['bad_gearbox_oil_temp'] = msg.bad_gearbox_oil_temp

        self.sms_flyeagle_a2rl_eav25_bsu_bad_powertrain_temp_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_bad_ride_front_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['bad_ride_height_front'] = msg.bad_ride_height_front
        sms_msg['bad_damper_stroke_f3rd'] = msg.bad_damper_stroke_f3rd
        sms_msg['bad_damper_stroke_fl'] = msg.bad_damper_stroke_fl
        sms_msg['bad_damper_stroke_fr'] = msg.bad_damper_stroke_fr

        self.sms_flyeagle_a2rl_eav25_bsu_bad_ride_front_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_bad_ride_rear_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['bad_ride_height_rear'] = msg.bad_ride_height_rear
        sms_msg['bad_damper_stroke_r3rd'] = msg.bad_damper_stroke_r3rd
        sms_msg['bad_damper_stroke_rl'] = msg.bad_damper_stroke_rl
        sms_msg['bad_damper_stroke_rr'] = msg.bad_damper_stroke_rr

        self.sms_flyeagle_a2rl_eav25_bsu_bad_ride_rear_pub.publish(sms_msg)
    
    def flyeagle_a2rl_eav25_bsu_bad_wheel_load_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['bad_load_wheel_fl'] = msg.bad_load_wheel_fl
        sms_msg['bad_load_wheel_fr'] = msg.bad_load_wheel_fr
        sms_msg['bad_load_wheel_rr'] = msg.bad_load_wheel_rr
        sms_msg['bad_load_wheel_rl'] = msg.bad_load_wheel_rl

        self.sms_flyeagle_a2rl_eav25_bsu_bad_wheel_load_pub.publish(sms_msg)
    
    def flyeagle_a2rl_eav25_bsu_bad_z_accel_body_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['bad_gz_body_front'] = msg.bad_gz_body_front
        sms_msg['bad_gz_body_rear'] = msg.bad_gz_body_rear

        self.sms_flyeagle_a2rl_eav25_bsu_bad_z_accel_body_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_bosch_pitch_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['mm710_pitch_rate'] = msg.mm710_pitch_rate
        sms_msg['mm710_hw_index'] = msg.mm710_hw_index
        sms_msg['mm710_pitch_rate_stat'] = msg.mm710_pitch_rate_stat
        sms_msg['mm710_az'] = msg.mm710_az
        sms_msg['mm710_msg_cnt_tx3'] = msg.mm710_msg_cnt_tx3
        sms_msg['mm710_az_stat'] = msg.mm710_az_stat
        sms_msg['mm710_crc_tx3'] = msg.mm710_crc_tx3
        
        self.sms_flyeagle_a2rl_eav25_bsu_bosch_pitch_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_bosch_roll_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['mm710_roll_rate'] = msg.mm710_roll_rate
        sms_msg['mm710_clu_stat5'] = msg.mm710_clu_stat5
        sms_msg['mm710_roll_rate_stat'] = msg.mm710_roll_rate_stat
        sms_msg['mm710_clu_diag'] = msg.mm710_clu_diag
        sms_msg['mm710_ax'] = msg.mm710_ax
        sms_msg['mm710_msg_cnt_tx2'] = msg.mm710_msg_cnt_tx2
        sms_msg['mm710_ax_stat'] = msg.mm710_ax_stat
        sms_msg['mm710_crc_tx2'] = msg.mm710_crc_tx2
        
        self.sms_flyeagle_a2rl_eav25_bsu_bosch_roll_pub.publish(sms_msg)
    
    def flyeagle_a2rl_eav25_bsu_bosch_yaw_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['mm710_yaw_rate'] = msg.mm710_yaw_rate
        sms_msg['mm710_clu_stat'] = msg.mm710_clu_stat
        sms_msg['mm710_yaw_rate_stat'] = msg.mm710_yaw_rate_stat
        sms_msg['mm710_temp_rate_z'] = msg.mm710_temp_rate_z
        sms_msg['mm710_ay'] = msg.mm710_ay
        sms_msg['mm710_msg_cnt_tx1'] = msg.mm710_msg_cnt_tx1
        sms_msg['mm710_ay_stat'] = msg.mm710_ay_stat
        sms_msg['mm710_crc_tx1'] = msg.mm710_crc_tx1
        
        self.sms_flyeagle_a2rl_eav25_bsu_bosch_yaw_pub.publish(sms_msg)
    
    def flyeagle_a2rl_eav25_bsu_brake_disk_temp_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['brake_disk_temp_fl'] = msg.brake_disk_temp_fl
        sms_msg['brake_disk_temp_fr'] = msg.brake_disk_temp_fr
        sms_msg['brake_disk_temp_rr'] = msg.brake_disk_temp_rr
        sms_msg['brake_disk_temp_rl'] = msg.brake_disk_temp_rl

        self.sms_flyeagle_a2rl_eav25_bsu_brake_disk_temp_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_bsu_ranges_01_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['cba_min_pressure_fl_pa'] = msg.cba_min_pressure_fl_pa
        sms_msg['cba_max_pressure_fl_pa'] = msg.cba_max_pressure_fl_pa
        sms_msg['cba_min_pressure_fr_pa'] = msg.cba_min_pressure_fr_pa
        sms_msg['cba_max_pressure_fr_pa'] = msg.cba_max_pressure_fr_pa
        sms_msg['cba_min_pressure_rl_pa'] = msg.cba_min_pressure_rl_pa
        sms_msg['cba_max_pressure_rl_pa'] = msg.cba_max_pressure_rl_pa
        sms_msg['cba_min_pressure_rr_pa'] = msg.cba_min_pressure_rr_pa
        sms_msg['cba_max_pressure_rr_pa'] = msg.cba_max_pressure_rr_pa
        sms_msg['ice_max_throttle'] = msg.ice_max_throttle
        sms_msg['ice_min_throttle'] = msg.ice_min_throttle
            
        self.sms_flyeagle_a2rl_eav25_bsu_bsu_ranges_01_pub.publish(sms_msg)
    
    def flyeagle_a2rl_eav25_bsu_bsu_ranges_02_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['psa_min_angle_rad'] = msg.psa_min_angle_rad
        sms_msg['psa_max_angle_rad'] = msg.psa_max_angle_rad
        sms_msg['psa_min_angle_speed_rad_s'] = msg.psa_min_angle_speed_rad_s
        sms_msg['psa_max_angle_speed_rad_s'] = msg.psa_max_angle_speed_rad_s
        sms_msg['psa_min_torque_m_nm'] = msg.psa_min_torque_m_nm
        sms_msg['psa_max_torque_m_nm'] = msg.psa_max_torque_m_nm

        self.sms_flyeagle_a2rl_eav25_bsu_bsu_ranges_02_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_bsu_status_01_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['bsu_switch_off_req'] = msg.bsu_switch_off_req
        sms_msg['bsu_limp_mode_activated'] = msg.bsu_limp_mode_activated
        sms_msg['bsu_hl_stop_request'] = msg.bsu_hl_stop_request
        sms_msg['bsu_hl_warning'] = msg.bsu_hl_warning
        sms_msg['bsu_em_stop_activated'] = msg.bsu_em_stop_activated
        sms_msg['bsu_ml_stop_activated'] = msg.bsu_ml_stop_activated
        sms_msg['bsu_alive_counter'] = msg.bsu_alive_counter
        sms_msg['bsu_status'] = msg.bsu_status
        sms_msg['abs_external_enable_ack'] = msg.abs_external_enable_ack

        self.sms_flyeagle_a2rl_eav25_bsu_bsu_status_01_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_cba_status_fl_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['cba_actual_pressure_fl_pa'] = msg.cba_actual_pressure_fl_pa
        sms_msg['cba_actual_pressure_fl'] = msg.cba_actual_pressure_fl
        sms_msg['cba_target_pressure_fl_ack'] = msg.cba_target_pressure_fl_ack
        sms_msg['cba_actual_current_fl_a'] = msg.cba_actual_current_fl_a
        sms_msg['cba_voltage_fl_v'] = msg.cba_voltage_fl_v
           
        self.sms_flyeagle_a2rl_eav25_bsu_cba_status_fl_pub.publish(sms_msg)
    
    def flyeagle_a2rl_eav25_bsu_cba_status_fr_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['cba_actual_pressure_fr_pa'] = msg.cba_actual_pressure_fr_pa
        sms_msg['cba_actual_pressure_fr'] = msg.cba_actual_pressure_fr
        sms_msg['cba_target_pressure_fr_ack'] = msg.cba_target_pressure_fr_ack
        sms_msg['cba_actual_current_fr_a'] = msg.cba_actual_current_fr_a
        sms_msg['cba_voltage_fr_v'] = msg.cba_voltage_fr_v

        self.sms_flyeagle_a2rl_eav25_bsu_cba_status_fr_pub.publish(sms_msg)
    
    def flyeagle_a2rl_eav25_bsu_cba_status_rl_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['cba_actual_pressure_rl_pa'] = msg.cba_actual_pressure_rl_pa
        sms_msg['cba_actual_pressure_rl'] = msg.cba_actual_pressure_rl
        sms_msg['cba_target_pressure_rl_ack'] = msg.cba_target_pressure_rl_ack
        sms_msg['cba_actual_current_rl_a'] = msg.cba_actual_current_rl_a
        sms_msg['cba_voltage_rl_v'] = msg.cba_voltage_rl_v

        self.sms_flyeagle_a2rl_eav25_bsu_cba_status_rl_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_cba_status_rr_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['cba_actual_pressure_rr_pa'] = msg.cba_actual_pressure_rr_pa
        sms_msg['cba_actual_pressure_rr'] = msg.cba_actual_pressure_rr
        sms_msg['cba_target_pressure_rr_ack'] = msg.cba_target_pressure_rr_ack
        sms_msg['cba_actual_current_rr_a'] = msg.cba_actual_current_rr_a
        sms_msg['cba_voltage_rr_v'] = msg.cba_voltage_rr_v
            
        self.sms_flyeagle_a2rl_eav25_bsu_cba_status_rr_pub.publish(sms_msg)
    
    def flyeagle_a2rl_eav25_bsu_diagnostic_word_01_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['bms_starting_faild'] = msg.bms_starting_faild
        sms_msg['bms_timeout_error'] = msg.bms_timeout_error
        sms_msg['cba_fl_counter_error'] = msg.cba_fl_counter_error
        sms_msg['cba_fl_derating'] = msg.cba_fl_derating
        sms_msg['cba_fl_error'] = msg.cba_fl_error
        sms_msg['cba_fl_timeout_error'] = msg.cba_fl_timeout_error
        sms_msg['cba_fr_counter_error'] = msg.cba_fr_counter_error
        sms_msg['cba_fr_derating'] = msg.cba_fr_derating
        sms_msg['cba_fr_error'] = msg.cba_fr_error
        sms_msg['cba_fr_timeout_error'] = msg.cba_fr_timeout_error
        sms_msg['cba_rl_counter_error'] = msg.cba_rl_counter_error
        sms_msg['cba_rl_derating'] = msg.cba_rl_derating
        sms_msg['cba_rl_error'] = msg.cba_rl_error
        sms_msg['cba_rl_timeout_error'] = msg.cba_rl_timeout_error
        sms_msg['cba_rr_counter_error'] = msg.cba_rr_counter_error
        sms_msg['cba_rr_derating'] = msg.cba_rr_derating
        sms_msg['cba_rr_error'] = msg.cba_rr_error
        sms_msg['cba_rr_timeout_error'] = msg.cba_rr_timeout_error
        sms_msg['dcdc_starting_faild'] = msg.dcdc_starting_faild
        sms_msg['dcdc_timeout_error'] = msg.dcdc_timeout_error
        sms_msg['ecu_timeout_em_fault'] = msg.ecu_timeout_em_fault
        sms_msg['dem_cbafl_not_receive'] = msg.dem_cbafl_not_receive
        sms_msg['ice_gear_low_oil_temp_warning'] = msg.ice_gear_low_oil_temp_warning
        sms_msg['ice_engine_off_rejected'] = msg.ice_engine_off_rejected
        sms_msg['dem_pdu12_v_not_receive'] = msg.dem_pdu12_v_not_receive
        sms_msg['ice_starting_fueling_failed'] = msg.ice_starting_fueling_failed
        sms_msg['ice_starting_oil_heater_failed'] = msg.ice_starting_oil_heater_failed
        sms_msg['ice_starting_starting_failed'] = msg.ice_starting_starting_failed
        sms_msg['ice_aps_warning'] = msg.ice_aps_warning
        sms_msg['hl_counter_error'] = msg.hl_counter_error
        sms_msg['hl_timeout_error'] = msg.hl_timeout_error
        sms_msg['ice_counter_error'] = msg.ice_counter_error
        sms_msg['ice_timeout_error'] = msg.ice_timeout_error
        sms_msg['dem_cbafr_not_receive'] = msg.dem_cbafr_not_receive
        sms_msg['ice_oil_temp_under_min_start_limit'] = msg.ice_oil_temp_under_min_start_limit
        sms_msg['pdu12_counter_error'] = msg.pdu12_counter_error
        sms_msg['pdu12_timeout_error'] = msg.pdu12_timeout_error
        sms_msg['pdu48_counter_error'] = msg.pdu48_counter_error
        sms_msg['pdu48_timeout_error'] = msg.pdu48_timeout_error
        sms_msg['dem_cbarl_not_receive'] = msg.dem_cbarl_not_receive
        sms_msg['psa_counter_error'] = msg.psa_counter_error
        sms_msg['psa_derating'] = msg.psa_derating
        sms_msg['psa_error'] = msg.psa_error
        sms_msg['psa_timeout_error'] = msg.psa_timeout_error
        sms_msg['dem_cbarr_not_receive'] = msg.dem_cbarr_not_receive
        sms_msg['em_stop_conditions_active'] = msg.em_stop_conditions_active
        sms_msg['ml_stop_conditions_active'] = msg.ml_stop_conditions_active
        sms_msg['dcdc4812_under_min_start_limit'] = msg.dcdc4812_under_min_start_limit
        sms_msg['ice_override_wrong_config'] = msg.ice_override_wrong_config
        sms_msg['bsu_wrong_init_config'] = msg.bsu_wrong_init_config
        sms_msg['ice_boost_warning'] = msg.ice_boost_warning
        sms_msg['ice_coolant_pressure_warning'] = msg.ice_coolant_pressure_warning
        sms_msg['ice_coolant_temperature_warning'] = msg.ice_coolant_temperature_warning
        sms_msg['ice_fuel_pressure_warning'] = msg.ice_fuel_pressure_warning
        sms_msg['ice_gear_oil_temperature_warning'] = msg.ice_gear_oil_temperature_warning
        sms_msg['ice_oil_pressure_warning'] = msg.ice_oil_pressure_warning
        sms_msg['ice_oil_temperature_warning'] = msg.ice_oil_temperature_warning
        sms_msg['pdu12_active_anti_fire'] = msg.pdu12_active_anti_fire
        sms_msg['ice_sensor_failure_warning'] = msg.ice_sensor_failure_warning
        sms_msg['ice_target_gear_not_reached_warning'] = msg.ice_target_gear_not_reached_warning
        sms_msg['ice_fuel_volume_warning'] = msg.ice_fuel_volume_warning
        sms_msg['rm_counter_error'] = msg.rm_counter_error
        sms_msg['rm_timeout_error'] = msg.rm_timeout_error
            
        self.sms_flyeagle_a2rl_eav25_bsu_diagnostic_word_01_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_diagnostic_word_02_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['rc_force_race_mode'] = msg.rc_force_race_mode
        sms_msg['rc_beacon_time_out'] = msg.rc_beacon_time_out
        sms_msg['display_timeout_error'] = msg.display_timeout_error
        sms_msg['rm_red_flag'] = msg.rm_red_flag
        sms_msg['rm_safe_stop'] = msg.rm_safe_stop
        sms_msg['display_counter_error'] = msg.display_counter_error
        sms_msg['cba_em_brake_sat_def_val'] = msg.cba_em_brake_sat_def_val
        sms_msg['cba_sat_values_def_val'] = msg.cba_sat_values_def_val
        sms_msg['ice_oil_temp_start_limit_def_val'] = msg.ice_oil_temp_start_limit_def_val
        sms_msg['ml_stop_brake_sat_def_val'] = msg.ml_stop_brake_sat_def_val
        sms_msg['psa_sat_values_def_val'] = msg.psa_sat_values_def_val
        sms_msg['limp_sat_values_def_val'] = msg.limp_sat_values_def_val
        sms_msg['fan_car_speed_max_value_def_val'] = msg.fan_car_speed_max_value_def_val
        sms_msg['fan_car_speed_min_value_def_val'] = msg.fan_car_speed_min_value_def_val
        sms_msg['fan_water_temp_max_value_def_val'] = msg.fan_water_temp_max_value_def_val
        sms_msg['fan_water_temp_min_value_def_val'] = msg.fan_water_temp_min_value_def_val
        sms_msg['heater_oil_temp_max_value_def_val'] = msg.heater_oil_temp_max_value_def_val
        sms_msg['ice_fuel_pres_start_limit_def_val'] = msg.ice_fuel_pres_start_limit_def_val
        sms_msg['ice_oil_temp_by_pass_def_val'] = msg.ice_oil_temp_by_pass_def_val
        sms_msg['psa_sat_rates_def_val'] = msg.psa_sat_rates_def_val
            
        self.sms_flyeagle_a2rl_eav25_bsu_diagnostic_word_02_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_em_status_01_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['dcdc4812_voltage_v'] = msg.dcdc4812_voltage_v
        sms_msg['em_status'] = msg.em_status
        sms_msg['hl_stop_deceleration_1'] = msg.hl_stop_deceleration_1
        sms_msg['hl_stop_deceleration_2'] = msg.hl_stop_deceleration_2
        sms_msg['hl_stop_time_to_dec_1'] = msg.hl_stop_time_to_dec_1
        sms_msg['hl_stop_time_to_dec_2'] = msg.hl_stop_time_to_dec_2

        self.sms_flyeagle_a2rl_eav25_bsu_em_status_01_pub.publish(sms_msg)
    
    def flyeagle_a2rl_eav25_bsu_hl_msg_01_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['hl_alive_01'] = msg.hl_alive_01
        sms_msg['hl_target_pressure_rr'] = msg.hl_target_pressure_rr
        sms_msg['hl_target_pressure_rl'] = msg.hl_target_pressure_rl
        sms_msg['hl_target_pressure_fr'] = msg.hl_target_pressure_fr
        sms_msg['hl_target_pressure_fl'] = msg.hl_target_pressure_fl
        sms_msg['hl_target_gear'] = msg.hl_target_gear
        sms_msg['hl_target_throttle'] = msg.hl_target_throttle

        self.sms_flyeagle_a2rl_eav25_bsu_hl_msg_01_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_hl_msg_02_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['hl_alive_02'] = msg.hl_alive_02
        sms_msg['hl_psa_mode_of_operation'] = msg.hl_psa_mode_of_operation
        sms_msg['hl_target_psa_control'] = msg.hl_target_psa_control
        sms_msg['hl_psa_profile_acc_rad_s2'] = msg.hl_psa_profile_acc_rad_s2
        sms_msg['hl_psa_profile_dec_rad_s2'] = msg.hl_psa_profile_dec_rad_s2
        sms_msg['hl_psa_profile_vel_rad_s'] = msg.hl_psa_profile_vel_rad_s

        self.sms_flyeagle_a2rl_eav25_bsu_hl_msg_02_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_hl_msg_03_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['hl_alive_03'] = msg.hl_alive_03
        sms_msg['hl_dbw_enable'] = msg.hl_dbw_enable
        sms_msg['hl_push_to_pass_on'] = msg.hl_push_to_pass_on
        sms_msg['hl_pdu12_activate_gnss'] = msg.hl_pdu12_activate_gnss
        sms_msg['hl_pdu12_activate_oss'] = msg.hl_pdu12_activate_oss
        sms_msg['hl_ice_enable'] = msg.hl_ice_enable
        sms_msg['hl_pdu12_activate_lidar'] = msg.hl_pdu12_activate_lidar
        sms_msg['hl_pdu12_activate_radar'] = msg.hl_pdu12_activate_radar
        sms_msg['ice_start_fuel_level_l'] = msg.ice_start_fuel_level_l
        sms_msg['hl_crancking_by_pass'] = msg.hl_crancking_by_pass
        sms_msg['hl_switch_off_ok'] = msg.hl_switch_off_ok

        self.sms_flyeagle_a2rl_eav25_bsu_hl_msg_03_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_hl_msg_04_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['hl_latitude'] = msg.hl_latitude
        sms_msg['hl_longitude'] = msg.hl_longitude

        self.sms_flyeagle_a2rl_eav25_bsu_hl_msg_04_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_hl_msg_05_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['hl_height'] = msg.hl_height
        sms_msg['hl_vel_east'] = msg.hl_vel_east
        sms_msg['hl_vel_north'] = msg.hl_vel_north
        sms_msg['hl_vel_up'] = msg.hl_vel_up

        self.sms_flyeagle_a2rl_eav25_bsu_hl_msg_05_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_hl_msg_06_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['hl_pc_temp'] = msg.hl_pc_temp
        sms_msg['hl_cpu_usage'] = msg.hl_cpu_usage
        sms_msg['hl_gpu_usage'] = msg.hl_gpu_usage
        sms_msg['hl_perception_acc'] = msg.hl_perception_acc
        sms_msg['hl_localization_acc'] = msg.hl_localization_acc
        sms_msg['hl_aggressiveness'] = msg.hl_aggressiveness
        sms_msg['hl_watts_consumed'] = msg.hl_watts_consumed

        self.sms_flyeagle_a2rl_eav25_bsu_hl_msg_06_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_ice_status_01_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['ice_actual_gear'] = msg.ice_actual_gear
        sms_msg['ice_target_gear_ack'] = msg.ice_target_gear_ack
        sms_msg['ice_actual_throttle'] = msg.ice_actual_throttle
        sms_msg['ice_target_throttle_ack'] = msg.ice_target_throttle_ack
        sms_msg['ice_push_to_pass_req'] = msg.ice_push_to_pass_req
        sms_msg['ice_push_to_pass_ack'] = msg.ice_push_to_pass_ack
        sms_msg['ice_water_press_k_pa'] = msg.ice_water_press_k_pa
        sms_msg['ice_available_fuel_l'] = msg.ice_available_fuel_l
        sms_msg['ice_downshift_available'] = msg.ice_downshift_available

        self.sms_flyeagle_a2rl_eav25_bsu_ice_status_01_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_ice_status_02_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['ice_oil_temp_deg_c'] = msg.ice_oil_temp_deg_c
        sms_msg['ice_engine_speed_rpm'] = msg.ice_engine_speed_rpm
        sms_msg['ice_fuel_press_k_pa'] = msg.ice_fuel_press_k_pa
        sms_msg['ice_water_temp_deg_c'] = msg.ice_water_temp_deg_c
        sms_msg['ice_oil_press_k_pa'] = msg.ice_oil_press_k_pa

        self.sms_flyeagle_a2rl_eav25_bsu_ice_status_02_pub.publish(sms_msg)       

    def flyeagle_a2rl_eav25_bsu_kistler_acc_body_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['acc_x_body'] = msg.acc_x_body
        sms_msg['acc_y_body'] = msg.acc_y_body
        sms_msg['acc_z_body'] = msg.acc_z_body

        self.sms_flyeagle_a2rl_eav25_bsu_kistler_acc_body_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_kistler_angle_vel_body_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['ang_vel_x_body'] = msg.ang_vel_x_body
        sms_msg['ang_vel_y_body'] = msg.ang_vel_y_body
        sms_msg['ang_vel_z_body'] = msg.ang_vel_z_body

        self.sms_flyeagle_a2rl_eav25_bsu_kistler_angle_vel_body_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_kistler_correvit_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['vel_x_cor'] = msg.vel_x_cor
        sms_msg['vel_y_cor'] = msg.vel_y_cor
        sms_msg['vel_cor'] = msg.vel_cor
        sms_msg['angle_cor'] = msg.angle_cor

        self.sms_flyeagle_a2rl_eav25_bsu_kistler_correvit_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_kistler_distance_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['distance'] = msg.distance

        self.sms_flyeagle_a2rl_eav25_bsu_kistler_distance_pub.publish(sms_msg)
##youwenti
    def flyeagle_a2rl_eav25_bsu_kistler_pitch_roll_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['pitch'] = msg.pitch
        sms_msg['roll'] = msg.roll
        # sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['radius'] = msg.radius

        # self.sms_flyeagle_a2rl_eav25_bsu_kistler_pitch_roll_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_kistler_status_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['cor_sensor_id'] = msg.cor_sensor_id
        sms_msg['cor_temperature'] = msg.cor_temperature
        sms_msg['cor_lamp_current'] = msg.cor_lamp_current
        sms_msg['cor_filter_setting'] = msg.cor_filter_setting
        sms_msg['cor_stst'] = msg.cor_stst
        sms_msg['cor_filter_off_on'] = msg.cor_filter_off_on
        sms_msg['cor_lamp_current_control'] = msg.cor_lamp_current_control
        sms_msg['cor_temperature_ok'] = msg.cor_temperature_ok
        sms_msg['cor_head_status'] = msg.cor_head_status
        sms_msg['cor_angle_switched_off'] = msg.cor_angle_switched_off
        sms_msg['cor_direction'] = msg.cor_direction
        sms_msg['cor_ang_vel_correction'] = msg.cor_ang_vel_correction
        sms_msg['cor_direction_motion'] = msg.cor_direction_motion
        sms_msg['cor_direction_mounting'] = msg.cor_direction_mounting
        sms_msg['cor_direction_head_is_valid'] = msg.cor_direction_head_is_valid
        sms_msg['cor_direction_head'] = msg.cor_direction_head

        self.sms_flyeagle_a2rl_eav25_bsu_kistler_status_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_kistler_vel_angle_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['vel_x'] = msg.vel_x
        sms_msg['vel_y'] = msg.vel_y
        sms_msg['vel'] = msg.vel
        sms_msg['angle'] = msg.angle

        self.sms_flyeagle_a2rl_eav25_bsu_kistler_vel_angle_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_pd_us_status_01_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['pdu12_power_supply_voltage_v'] = msg.pdu12_power_supply_voltage_v
        sms_msg['pdu12_total_current_a'] = msg.pdu12_total_current_a
        sms_msg['pdu48_power_supply_voltage_v'] = msg.pdu48_power_supply_voltage_v
        sms_msg['pdu48_total_current_a'] = msg.pdu48_total_current_a

        self.sms_flyeagle_a2rl_eav25_bsu_pd_us_status_01_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_pit_packet_0_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['pitot_front_press'] = msg.pitot_front_press
        sms_msg['pitot_yaw_press'] = msg.pitot_yaw_press
        sms_msg['pitot_yaw_angle'] = msg.pitot_yaw_angle
        sms_msg['pitot_absolute_press'] = msg.pitot_absolute_press

        self.sms_flyeagle_a2rl_eav25_bsu_pit_packet_0_pub.publish(sms_msg)

    def flyeagle_a2rl_eav25_bsu_pit_packet_1_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['pitot_air_temp'] = msg.pitot_air_temp
        sms_msg['pitot_board_temp'] = msg.pitot_board_temp

        self.sms_flyeagle_a2rl_eav25_bsu_pit_packet_1_pub.publish(sms_msg)
    
    def flyeagle_a2rl_eav25_bsu_psa_status_01_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['psa_actual_pos_rad'] = msg.psa_actual_pos_rad
        sms_msg['psa_actual_speed_rad_s'] = msg.psa_actual_speed_rad_s
        sms_msg['psa_actual_torque_m_nm'] = msg.psa_actual_torque_m_nm
        sms_msg['psa_actual_mode_of_operation'] = msg.psa_actual_mode_of_operation
        sms_msg['psa_actual_current_a'] = msg.psa_actual_current_a
        sms_msg['psa_actual_voltage_v'] = msg.psa_actual_voltage_v

        self.sms_flyeagle_a2rl_eav25_bsu_psa_status_01_pub.publish(sms_msg)   

    def flyeagle_a2rl_eav25_bsu_psa_status_02_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['psa_target_psa_control_ack'] = msg.psa_target_psa_control_ack
        sms_msg['psa_actual_pos'] = msg.psa_actual_pos
        sms_msg['psa_actual_speed'] = msg.psa_actual_speed
        sms_msg['psa_actual_torque'] = msg.psa_actual_torque

        self.sms_flyeagle_a2rl_eav25_bsu_psa_status_02_pub.publish(sms_msg)   

    def flyeagle_a2rl_eav25_bsu_rm_status_01_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['rm_sector_flag'] = msg.rm_sector_flag
        sms_msg['rm_session_type'] = msg.rm_session_type
        sms_msg['rm_car_flag'] = msg.rm_car_flag
        sms_msg['rm_track_flag'] = msg.rm_track_flag

        self.sms_flyeagle_a2rl_eav25_bsu_rm_status_01_pub.publish(sms_msg)  

    def flyeagle_a2rl_eav25_bsu_sw_versions_01_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['bsu_sw_minor_version'] = msg.bsu_sw_minor_version
        sms_msg['bsu_sw_major_version'] = msg.bsu_sw_major_version
        sms_msg['bsu_sw_build_version'] = msg.bsu_sw_build_version
        sms_msg['psa_sw_minor_version'] = msg.psa_sw_minor_version
        sms_msg['psa_sw_major_version'] = msg.psa_sw_major_version
        sms_msg['psa_sw_build_version'] = msg.psa_sw_build_version
        sms_msg['pdu12_sw_version'] = msg.pdu12_sw_version
        sms_msg['pdu48_sw_version'] = msg.pdu48_sw_version

        self.sms_flyeagle_a2rl_eav25_bsu_sw_versions_01_pub.publish(sms_msg)  

    def flyeagle_a2rl_eav25_bsu_sw_versions_02_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['cba_fl_sw_minor_version'] = msg.cba_fl_sw_minor_version
        sms_msg['cba_fl_sw_major_version'] = msg.cba_fl_sw_major_version
        sms_msg['cba_fl_sw_build_version'] = msg.cba_fl_sw_build_version
        sms_msg['cba_fr_sw_minor_version'] = msg.cba_fr_sw_minor_version
        sms_msg['cba_fr_sw_major_version'] = msg.cba_fr_sw_major_version
        sms_msg['cba_fr_sw_build_version'] = msg.cba_fr_sw_build_version

        self.sms_flyeagle_a2rl_eav25_bsu_sw_versions_02_pub.publish(sms_msg)  

    def flyeagle_a2rl_eav25_bsu_sw_versions_03_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['cba_rl_sw_minor_version'] = msg.cba_rl_sw_minor_version
        sms_msg['cba_rl_sw_major_version'] = msg.cba_rl_sw_major_version
        sms_msg['cba_rl_sw_build_version'] = msg.cba_rl_sw_build_version
        sms_msg['cba_rr_sw_minor_version'] = msg.cba_rr_sw_minor_version
        sms_msg['cba_rr_sw_major_version'] = msg.cba_rr_sw_major_version
        sms_msg['cba_rr_sw_build_version'] = msg.cba_rr_sw_build_version

        self.sms_flyeagle_a2rl_eav25_bsu_sw_versions_03_pub.publish(sms_msg)  

    def flyeagle_a2rl_eav25_bsu_tpms_front_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['tpr4_temp_fl'] = msg.tpr4_temp_fl
        sms_msg['tpr4_temp_fr'] = msg.tpr4_temp_fr
        sms_msg['tpr4_abs_press_fl'] = msg.tpr4_abs_press_fl
        sms_msg['tpr4_abs_press_fr'] = msg.tpr4_abs_press_fr

        self.sms_flyeagle_a2rl_eav25_bsu_tpms_front_pub.publish(sms_msg)  

    def flyeagle_a2rl_eav25_bsu_tpms_rear_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['tpr4_temp_rl'] = msg.tpr4_temp_rl
        sms_msg['tpr4_temp_rr'] = msg.tpr4_temp_rr
        sms_msg['tpr4_abs_press_rl'] = msg.tpr4_abs_press_rl
        sms_msg['tpr4_abs_press_rr'] = msg.tpr4_abs_press_rr

        self.sms_flyeagle_a2rl_eav25_bsu_tpms_rear_pub.publish(sms_msg)  

    def flyeagle_a2rl_eav25_bsu_tyre_surface_temp_front_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['outer_fl'] = msg.outer_fl
        sms_msg['center_fl'] = msg.center_fl
        sms_msg['inner_fl'] = msg.inner_fl
        sms_msg['outer_fr'] = msg.outer_fr
        sms_msg['center_fr'] = msg.center_fr
        sms_msg['inner_fr'] = msg.inner_fr

        self.sms_flyeagle_a2rl_eav25_bsu_tyre_surface_temp_front_pub.publish(sms_msg)  

    def flyeagle_a2rl_eav25_bsu_tyre_surface_temp_rear_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['outer_rl'] = msg.outer_rl
        sms_msg['center_rl'] = msg.center_rl
        sms_msg['inner_rl'] = msg.inner_rl
        sms_msg['outer_rr'] = msg.outer_rr
        sms_msg['center_rr'] = msg.center_rr
        sms_msg['inner_rr'] = msg.inner_rr

        self.sms_flyeagle_a2rl_eav25_bsu_tyre_surface_temp_rear_pub.publish(sms_msg)  

    def flyeagle_a2rl_eav25_bsu_wheels_speed_01_callback(self, msg):
        sms_msg = def_msg('std_msgs::Null')
        sms_msg['timestamp'] =  msg.timestamp.nanoseconds / 1e9
        sms_msg['wss_speed_fl_rad_s'] = msg.wss_speed_fl_rad_s
        sms_msg['wss_speed_fr_rad_s'] = msg.wss_speed_fr_rad_s
        sms_msg['wss_speed_rl_rad_s'] = msg.wss_speed_rl_rad_s
        sms_msg['wss_speed_rr_rad_s'] = msg.wss_speed_rr_rad_s

        self.sms_flyeagle_a2rl_eav25_bsu_wheels_speed_01_pub.publish(sms_msg)  


if __name__ == '__main__':
    parser = argparse.ArgumentParser()
    parser.add_argument(
        '--config',
        type=str,
        default='default_params.json',
        help='SpireCV2 Config (.json)')
    parser.add_argument(
        '--job-name',
        type=str,
        default='live',
        help='SpireCV Job Name')
    parser.add_argument(
        '--ip',
        type=str,
        default='127.0.0.1',
        help='SpireMS Core IP')
    parser.add_argument(
        '--port',
        type=int,
        default=9094,
        help='SpireMS Core Port')
    args, unknown_args = parser.parse_known_args()
    if not os.path.isabs(args.config):
        current_path = os.path.abspath(__file__)
        params_dir = os.path.join(current_path[:current_path.find('spirecv-pro') + 11], 'params', 'spirecv2')
        args.config = os.path.join(params_dir, args.config)
    print("--config:", args.config)
    print("--job-name:", args.job_name)
    extra = get_extra_args(unknown_args)

    rclpy.init()
    node = ROS2A2RLTopic2SMSNode(args.job_name, param_dict_or_file=args.config, ip=args.ip, port=args.port, **extra)
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()
